#include "algoRotMat.h"
/*-----------------------------------------
创建于2019/7/24
By Fyra-BH
文件名:algoRotMat.c

描述:旋转矩阵的可视化测试
---------------------------------------
//坐标点如图：
//								 A ******************* B
//									*|                ** 														↑z
//								 * |               * *														|
//							C ******************* D*														|
//								*  |              *  *														|
//								*  |              *  *                     <------O
//								*  |              *  *                     x       ↘
//								*  |______________*__*                               ↘
//								* /A1             * *B1                                y
//								*                 **
//							C1*******************D1

------------------------------------------*/

/* 外部导入方法 */
extern point_t ref_O; // 参考原点
extern cube_t cb;
extern void LCD_DrawLine(float x1, float y1, float x2, float y2, uint16_t color);

/* 本文件常量 */
point_t ref_O = {100, 100}; // 屏幕坐标偏移
extern cube_t cb;                 // 全局变量
uint16_t BLACK = 0xffff;
uint16_t POINT_COLOR = 0xffff;
uint16_t CUBE_COLOR = 0x1234;

mat3x3_t mat_multi(mat3x3_t *mat1, mat3x3_t *mat2) // 3x3矩阵乘法
{
  mat3x3_t mat;
  mat.m1[0] = mat1->m1[0] * mat2->m1[0] + mat1->m1[1] * mat2->m2[0] + mat1->m1[2] * mat2->m3[0];
  mat.m1[1] = mat1->m1[0] * mat2->m1[1] + mat1->m1[1] * mat2->m2[1] + mat1->m1[2] * mat2->m3[1];
  mat.m1[2] = mat1->m1[0] * mat2->m1[2] + mat1->m1[1] * mat2->m2[2] + mat1->m1[2] * mat2->m3[2];

  mat.m2[0] = mat1->m2[0] * mat2->m1[0] + mat1->m2[1] * mat2->m2[0] + mat1->m2[2] * mat2->m3[0];
  mat.m2[1] = mat1->m2[0] * mat2->m1[1] + mat1->m2[1] * mat2->m2[1] + mat1->m2[2] * mat2->m3[1];
  mat.m2[2] = mat1->m2[0] * mat2->m1[2] + mat1->m2[1] * mat2->m2[2] + mat1->m2[2] * mat2->m3[2];

  mat.m3[0] = mat1->m3[0] * mat2->m1[0] + mat1->m3[1] * mat2->m2[0] + mat1->m3[2] * mat2->m3[0];
  mat.m3[1] = mat1->m3[0] * mat2->m1[1] + mat1->m3[1] * mat2->m2[1] + mat1->m3[2] * mat2->m3[1];
  mat.m3[2] = mat1->m3[0] * mat2->m1[2] + mat1->m3[1] * mat2->m2[2] + mat1->m3[2] * mat2->m3[2];
  // （貌似太硬核了。。。）
  return mat;
}

void mat_rest(mat3x3_t *mat) // 复位一个矩阵
{
  mat->m1[0] = 0;
  mat->m1[1] = 0;
  mat->m1[2] = 0;
  mat->m2[0] = 0;
  mat->m2[1] = 0;
  mat->m2[2] = 0;
  mat->m3[0] = 0;
  mat->m3[1] = 0;
  mat->m3[2] = 0;
}

void print_matrix(mat3x3_t mat) // 打印一个矩阵
{
  printf("%f\t%f\t%f\r\n", mat.m1[0], mat.m1[1], mat.m1[2]);
  printf("%f\t%f\t%f\r\n", mat.m2[0], mat.m2[1], mat.m2[2]);
  printf("%f\t%f\t%f\r\n", mat.m3[0], mat.m3[1], mat.m3[2]);
}

void draw_proj_line(point_t p1, point_t p2, unsigned short color) // 画出线的投影(默认投影面为XOY面)
{
  // 注意：要加上坐标偏移（屏幕的原点在左上角）
  POINT_COLOR = color;
  LCD_DrawLine(p1.x + ref_O.x, p1.y + ref_O.y,
               p2.x + ref_O.x, p2.y + ref_O.y,
               color);
}

void draw_visibel_line(point_t p1, point_t p2, unsigned long color) // 画出线的可视部分(默认投影面为XOY面)
{
}


void draw_proj_cube(cube_t c) // 画出立方体的投影(默认投影面为XOY面)
{
  // 画出各条棱
  draw_proj_line(c.A, c.B, CUBE_COLOR);
  draw_proj_line(c.A, c.C, CUBE_COLOR);
  draw_proj_line(c.D, c.C, CUBE_COLOR);
  draw_proj_line(c.D, c.B, CUBE_COLOR);

  draw_proj_line(c.A1, c.B1, CUBE_COLOR);
  draw_proj_line(c.A1, c.C1, CUBE_COLOR);
  draw_proj_line(c.D1, c.C1, CUBE_COLOR);
  draw_proj_line(c.D1, c.B1, CUBE_COLOR);

  draw_proj_line(c.A, c.A1, CUBE_COLOR);
  draw_proj_line(c.B, c.B1, CUBE_COLOR);
  draw_proj_line(c.C, c.C1, CUBE_COLOR);
  draw_proj_line(c.D, c.D1, CUBE_COLOR);
}

void draw_visibel_cube(cube_t c) // 画出立方体的可视部分(默认投影面为XOY面)
{
  // 待更新
}

void erase_proj_cube(cube_t c) // 擦除立方体的投影(默认投影面为XOY面)
{
  // 用背景色画线
  draw_proj_line(c.A, c.B, BLACK);
  draw_proj_line(c.A, c.C, BLACK);
  draw_proj_line(c.D, c.C, BLACK);
  draw_proj_line(c.D, c.B, BLACK);

  draw_proj_line(c.A1, c.B1, BLACK);
  draw_proj_line(c.A1, c.C1, BLACK);
  draw_proj_line(c.D1, c.C1, BLACK);
  draw_proj_line(c.D1, c.B1, BLACK);

  draw_proj_line(c.A, c.A1, BLACK);
  draw_proj_line(c.B, c.B1, BLACK);
  draw_proj_line(c.C, c.C1, BLACK);
  draw_proj_line(c.D, c.D1, BLACK);
}

void creat_center_cube(cube_t *c, float len) // 生成一个中心立方体
{
  c->A.x = len / 2.0f;
  c->A.y = -len / 2.0f;
  c->A.z = len / 2.0f;

  c->A1 = c->A;
  c->A1.z = -len / 2.0f;

  c->B = c->A;
  c->B.x = -len / 2.0f;

  c->B1 = c->B;
  c->B1.z = -len / 2.0f;

  c->C.x = len / 2.0f;
  c->C.y = len / 2.0f;
  c->C.z = len / 2.0f;

  c->C1 = c->C;
  c->C1.z = -len / 2.0f;

  c->D = c->C;
  c->D.x = -len / 2.0f;

  c->D1 = c->D;
  c->D1.z = -len / 2.0f;
}

void rotate_point(point_t *p, float roll, float pitch, float yaw) // 旋转一个点
{
  mat3x3_t mat_x, mat_y, mat_z, mat_temp;
  point_t p_temp;
  // 清空三个轴的旋转矩阵
  mat_rest(&mat_x);
  mat_rest(&mat_y);
  mat_rest(&mat_z);

  mat_x.m1[0] = 1.0f;
  mat_x.m2[1] = cos(roll);
  mat_x.m2[2] = sin(roll);
  mat_x.m3[1] = -sin(roll);
  mat_x.m3[2] = cos(roll);

  mat_y.m1[0] = cos(pitch);
  mat_y.m1[2] = -sin(pitch);
  mat_y.m2[1] = 1.0f;
  mat_y.m3[0] = sin(pitch);
  mat_y.m3[2] = cos(pitch);

  mat_z.m1[0] = cos(yaw);
  mat_z.m1[1] = -sin(yaw);
  mat_z.m2[0] = sin(yaw);
  mat_z.m2[1] = cos(yaw);
  mat_z.m3[2] = 1.0f;

  mat_temp = mat_multi(&mat_x, &mat_z);
  mat_temp = mat_multi(&mat_y, &mat_temp);

  p_temp.x = mat_temp.m1[0] * p->x +
             mat_temp.m1[1] * p->y +
             mat_temp.m1[2] * p->z;

  p_temp.y = mat_temp.m2[0] * p->x +
             mat_temp.m2[1] * p->y +
             mat_temp.m2[2] * p->z;

  p_temp.z = mat_temp.m3[0] * p->x +
             mat_temp.m3[1] * p->y +
             mat_temp.m3[2] * p->z;

  p->x = p_temp.x;
  p->y = p_temp.y;
  p->z = p_temp.z;
}

void rotate_cube(cube_t *cb, float roll, float pitch, float yaw) // 旋转一个立方体
{
  rotate_point(&cb->A, roll, pitch, yaw);
  rotate_point(&cb->B, roll, pitch, yaw);
  rotate_point(&cb->C, roll, pitch, yaw);
  rotate_point(&cb->D, roll, pitch, yaw);
  rotate_point(&cb->A1, roll, pitch, yaw);
  rotate_point(&cb->B1, roll, pitch, yaw);
  rotate_point(&cb->C1, roll, pitch, yaw);
  rotate_point(&cb->D1, roll, pitch, yaw);
}
